//
// Created by PengYH on 2024/8/11.
//

#ifndef CPP_TEST_SRC_SRC_APP_HLP_RADAR_TEST_ADAPTIVEFILTER_H_
#define CPP_TEST_SRC_SRC_APP_HLP_RADAR_TEST_ADAPTIVEFILTER_H_

#include <cmath>

#include <iostream>
#include <vector>
#include <algorithm>
#include <numeric>

#include "log/log.h"

// 自适应滤波器类
class AdaptiveFilter {
 public:
  AdaptiveFilter(size_t windowSize, double initialWeight = 0.5)
      : win_size_(windowSize), weight_(initialWeight) {}

  // 处理新数据并返回滤波后的结果
  double process(double newData) {
    // 将新数据添加到窗口
    if (win_data_.size() >= win_size_) {
      win_data_.erase(win_data_.begin());
    }
    win_data_.push_back(newData);

    // 计算加权平均值
    double weightedSum = 0.0;
    double weightSum = 0.0;

    for (size_t i = 0; i < win_data_.size(); ++i) {
      double currentWeight = weight_ * (1.0 - (static_cast<double>(i) / win_data_.size()));
      weightedSum += win_data_[i] * currentWeight;
      weightSum += currentWeight;
    }

    double filteredValue = weightedSum / weightSum;

    // 自适应调整权重（这里简单地使用标准差作为调整依据）
    std_dev_ = calculateStandardDeviation();
    weight_ = 0.5 + 0.5 * (1.0 / (1.0 + std_dev_)); // 根据标准差动态调整权重
    // LOGD("std_dev %f, weight %f", std_dev_, weight_);
    return filteredValue;
  }

  bool CheckIsMotion(double nowData) {
//    LOGD("std_dev %f", std_dev_);
    if (std_dev_ < 20) {
      return false;
    }

    for (int i = 0; i < win_data_.size(); ++i) {
      int diff = abs(win_data_[i] - nowData);
      if (diff > (std_dev_+10)) {
        return true;
      }
    }
    return false;
  }

  // 计算数据窗口的标准差
  double calculateStandardDeviation() const {
    double mean = std::accumulate(win_data_.begin(),
                                  win_data_.end(), 0.0) / win_data_.size();
    double variance = 0.0;
    for (double value : win_data_) {
      variance += (value - mean) * (value - mean);
    }
    variance /= win_data_.size();
    return std::sqrt(variance);
  }

  double GetLastData() {
    return (*win_data_.end().base());
  }

 private:
  double weight_;
  double std_dev_;
  size_t win_size_;
  std::vector<double> win_data_;
};

#endif //CPP_TEST_SRC_SRC_APP_HLP_RADAR_TEST_ADAPTIVEFILTER_H_
